package com.dkts.oil.test.path;


import org.locationtech.jts.geom.Coordinate;

import java.util.*;

/**
 * 网格路径规划（A*）
 */
public class GridPathAStart {


    public static List<Coordinate> findPath(TempGridInfo[][] grid, TempGridInfo start, TempGridInfo end) {
        PriorityQueue<TempGridInfo> openSet = new PriorityQueue<>(Comparator.comparingDouble(TempGridInfo::getTotalCost));
        Set<String> closedSet = new HashSet<>();

        openSet.offer(start);

        while (!openSet.isEmpty()) {
            TempGridInfo current = openSet.poll();
//             System.out.println("Current: " + current.indexX + "," + current.indexY);

            if (current.indexX == end.indexX && current.indexY == end.indexY) {
               return printPath(current);
            }

            closedSet.add(current.indexX + "," + current.indexY);

            for (double[] dir : new double[][]{{0, 1}, {1, 0}, {0, -1}, {-1, 0}}) {
                int newX = current.indexX + (int) dir[0];
                int newY = current.indexY + (int) dir[1];
                if (isValidMove(newX, newY, grid, closedSet)) {
                    TempGridInfo neighbor = new TempGridInfo(grid[newX][newY].code, 0, grid[newX][newY].x, grid[newX][newY].y);
                    neighbor.cost = current.cost + STEP_COST;
                    neighbor.parent = current;
                    neighbor.indexX = newX;
                    neighbor.indexY = newY;
                    openSet.offer(neighbor);
                }
            }
        }

        System.out.println("No path found.");
        return new ArrayList<>();
    }

    private static final double STEP_COST = 1;

    private static boolean isValidMove(int indexX, int indexY, TempGridInfo[][] grid, Set<String> closedSet) {
        return indexX >= 0 && indexX < grid.length &&
                indexY >= 0 && indexY < grid[0].length &&
                grid[indexX][indexY].isAdvice == 0 &&
                !closedSet.contains(indexX + "," + indexY);
    }

    private static List<Coordinate> printPath(TempGridInfo end) {
        LinkedList<TempGridInfo> path = new LinkedList<>();
        TempGridInfo current = end;


        while (current != null) {
            path.addFirst(current);
            current = current.parent;
        }
        List<Coordinate> coordinates = new ArrayList<>();
        System.out.println("Path:");
        for (TempGridInfo p : path) {
            coordinates.add(new Coordinate(p.x,p.y));
            System.out.println("Grid Code: " + p.getGridCode());
        }

        return coordinates;
    }
}
